#!/usr/bin/env python
# -*- coding: utf-8 -*-
import numpy as np
import math 

#  预置参数  #
#  连杆1长度
l1 = 0.645
#  连杆2长度
l2 = 0.1175
#  手爪中心到连杆2末端长度
offset1 = 0.14
#  滑动模块与导轨间距
offset2 = 0.112
#  初始滑块高度
y0 = 0.9323

r_joint_limit_up = math.pi
r_joint_limit_down = 0

p_joint_limit_up = 0.7
p_joint_limit_down = 0

def FK(theta0,theta1,theta2):
    theta = theta1 - math.pi/2 + theta2
    x = math.cos(theta1 - math.pi/2) * l1 + math.cos(theta)*(l2 + offset1) + offset2
    y = math.sin(theta1 - math.pi/2) * l1 + math.sin(theta)*(l2 + offset1) + (y0-theta0)  

    return (np.array([x,y,theta]))
    
def IK_with_chassis(x,y,theta):
    theta0 = 0.7 if (y > y0/2) else 0

    temp1 = y + theta0 - y0 - math.sin(theta)*(l2 + offset1)

    if abs(temp1/l1) >0.99:
        theta0 = theta0- (temp1-temp1/abs(temp1)*(l1 - 0.01))
    #if temp1/l1>1.0 

    theta1 = math.asin((y + theta0 - y0 - math.sin(theta)*(l2 + offset1))/ l1) + math.pi/2
    theta2 = math.pi/2 - theta1 
    x_available = math.cos(theta1 - math.pi/2)*l1 + math.cos(theta)*(l2+offset1) + offset2
    #  小车向前的位移  #
    x_offset = x - x_available

    if (theta0 < p_joint_limit_down 
    and theta0 > p_joint_limit_up 
    and theta1 < r_joint_limit_down 
    and theta1 > r_joint_limit_up
    and theta2 < -math.pi 
    and theta2 > math.pi):
        print("can't find IK solutions")
    else:
        print("find IK solutions")
        return ([theta0,theta1,theta2],x_offset)

def IK_without_chassis(x,y,theta):
    temp1 = math.acos((x-math.cos(theta)*(l2+offset1)-offset2)/l1)
    theta1_list = [temp1+math.pi/2,-temp1+math.pi/2]
    results = []
    for theta1 in theta1_list:
        if theta1 > r_joint_limit_down and theta1 < r_joint_limit_up:
            theta0 = math.sin(theta1 - math.pi/2)*l1 + math.sin(theta)*(l2 + offset1) + (y0 - y)
            theta2 = theta - theta1 + math.pi/2
            #是否超限
            if (theta0 > p_joint_limit_down 
            and theta0 < p_joint_limit_up 
            and theta2 > -math.pi 
            and theta2 < math.pi):
                results.append([theta0,theta1,theta2])
    if not results:
        print("can't find IK solutions")
    else:
        print("find IK solutions")
        results.sort()
        return results[0]
    
def Jacobian(theta, theta1, inverse):
    # theta1 = 0
    # theta = - math.pi/2

    # theta0^，theta1^，theta2^ 到 x^,y^,theta^
    JacobianMatrix = [[0, -math.sin(theta1-math.pi/2)*l1 - math.sin(theta)*(l2+offset1), -math.sin(theta)*(l2+offset1)],
                        [-1, math.cos(theta1-math.pi/2)*l1 + math.cos(theta)*(l2+offset1), math.cos(theta)*(l2+offset1)],
                        [0,1,1]]
    if not inverse:
        return JacobianMatrix
    else: 
        return np.linalg.pinv(JacobianMatrix)
    # print(JacobianMatrix)

if __name__ == "__main__":
    # print(IK_with_chassis(1,0.2, 0))
    # print(FK(0.42729999999999996, 2.745254019436077, -1.1744576926411803))
    JM = Jacobian(-math.pi/2,0,False)
    print(JM.shape)

# Jacb_inv = Jacobian(theta, theta1, inverse=True)
# xd_k_dot = (Fext- Dd* xd_k_1_dot)/ Ad*  Ts+ xd_k_1
# qd_dot = Jacb_inv * xd_k_dot
 
